#include <ch9.h>
#include <Max3421e.h>
#include <Max3421e_constants.h>
#include <Max_LCD.h>
#include <Usb.h>
#include <Encoder.h>
#include <PID_v1.h>

#include <AndroidAccessory.h>
Encoder actuator(18, 19); // 18 is red, 19 is black, tied to interrupt pins on MEGA
Encoder actuator2(20, 21); // 20 is red, 21 is black, tied to interrupt pins on MEGA

AndroidAccessory acc("Manufacturer",
"Model",
"Description",
"1.0",
"edu.uidaho.RG",
"0000000012345678");

int sensorPin = A0;    // select the input pin for the potentiometer
int sensorPin2 = A2;
int ledPin = 13;      // select the pin for the LED - onboard arduino LED
int sensorValue = 0;
int sensorValue2 = 0;
long encoder_range;
long encoder_range2;
int ACT1 = 9; //pin 9 for actuator 1
int ACT2 = 10;//pin 10 for actuator 2
int SWITCH = 24;
int SWITCH2 = 48;

//Define Variables we'll be connecting to
double Setpoint = 50, Input, Output;
double Setpoint2 = 50, Input2, Output2;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,5,1,.5, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2,5,1,.5, DIRECT);

//Feedback LEDs
int LED1 = 30;
int LED2 = 44;

void setup() {

  // declare the ledPin as an OUTPUT:

  Serial.begin(115200);
  //Serial.println("Start");


  //  pinMode(ledPin, OUTPUT);
  myPID.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);

  //TCCR3B = TCCR3B & 0b1111000 | 0x01; // 5 is on timer 3
  TCCR2B = TCCR2B & 0b1111000 | 0x01;  // 9 and 10 are on timer 2 see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1235060559/15
  //Setup the Feedback LEDs for output and ground the cathode
  pinMode(LED1, OUTPUT);
  pinMode(31, OUTPUT); 
  digitalWrite(31,LOW);
  pinMode(LED2, OUTPUT);
  pinMode(45, OUTPUT); 
  pinMode(22, OUTPUT); 
  pinMode(26, OUTPUT); 
  pinMode(SWITCH, INPUT);
  pinMode(SWITCH2,INPUT);
  pinMode(46,OUTPUT);
  digitalWrite(45,LOW);
  digitalWrite(46,LOW);
  digitalWrite(22,LOW);
  digitalWrite(26,LOW);
  acc.powerOn();

  //calibrate end points of claw range of motion, 
  //grab encoder values at these boundary points
  ramp(-1,1); // close finger
  actuator.write(0); //set initial minimum val from encoder 1
  digitalWrite(LED1, LOW);
  Serial.print("read value (closed): ");
  Serial.println(actuator.read());
  analogWrite(ACT1,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)
  ramp(-1,2); // close finger
  actuator2.write(0); //set initial minimum val from encoder 1
  digitalWrite(LED2, LOW);
  analogWrite(ACT2,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)
  //  Serial.print("read value (closed): ");
  //  Serial.println(actuator.read());
  ramp(1,1); // open finger from act 1
  //  Serial.print("read value (open): ");
  //  Serial.println(actuator.read());

  encoder_range = actuator.read();//set other end point, max val from encoder 1
  Serial.print("read value encoder range: ");
  Serial.println(encoder_range);

  digitalWrite(LED1, LOW);
  analogWrite(ACT1,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)
  ramp(1,2); // open finger from act 1
  encoder_range2 = actuator2.read();//set other end point, max val from encoder 2
  digitalWrite(LED2, LOW);
  //  Serial.print("read value encoder range: ");
  //  Serial.println(encoder_range);

  analogWrite(ACT1,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)
  analogWrite(ACT2,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)

  myPID.SetOutputLimits(5, 250); // Guess: the actuator doesn't like a PWM of 0% or 100%?
  myPID2.SetOutputLimits(5, 250); // Guess: the actuator doesn't like a PWM of 0% or 100%?
  myPID.SetSampleTime(5); // Set the PID to compute every 5 ms
  myPID2.SetSampleTime(5); // Set the PID to compute every 5 ms
}

void loop()
{
  long newAct;
  long newAct2;
  int writeval2;
  int writeval;
  int MSGLEN = 11;

  byte msgOut[5]; // Output Fixed length message of format 0XXCC (Zero-Variable-Variable-Control-Control)
  byte msgIn[11]; // Input Fixed length message of format 0XX[4][4] (Zero-Variable-Variable-int-int)
  int inputint[2]; //array of the two inputs
  float received_setpoint[2]; // setpoint scaled between 0 and 100.00 (floating point)

  //Get potentiometer input and scale it
  //sensorValue = analogRead(sensorPin);
  //sensorValue2 = analogRead(sensorPin2);
  //sensorValue = map(sensorValue, 0, 1023, 1, 125); // change?
  // sensorValue2 = map(sensorValue2, 0, 1023, 1, 125);

  if (acc.isConnected())
  {
    //Serial.println("Acc successfully conneced");

    newAct = actuator.read();
    newAct2 = actuator2.read();

    // For Actuator 1
    Input = newAct*100.0/encoder_range; //Scales from 0 to 100
    int currentActPosition = newAct*125/encoder_range;
    currentActPosition = constrain(currentActPosition,1,125);

    //For Actuator 2
    Input2 = newAct2*100.0/encoder_range2; //Scales from 0 to 100
    int currentAct2Position = newAct2*125/encoder_range2;
    currentAct2Position = constrain(currentAct2Position,1,125);


    /*******
     *RECEIVE
     ******/
    int len = acc.read(msgIn, sizeof(msgIn), MSGLEN); // read data into msgIn variable [ 0, x, x, 4byte, 4byte ]
    if (len > MSGLEN - 1) { 
      Serial.println("Length > 0");
      if (msgIn[0] == 0){ // Check correct protocol starting value of 0 (Zero)
        //Light two feedback LEDs corresponding to input received
        if (msgIn[1] == 1) digitalWrite(LED1,HIGH); // turn on light
        else digitalWrite(LED1,LOW); // turn off light

        if (msgIn[2] == 1)digitalWrite(LED2,HIGH); // turn on light
        else digitalWrite(LED2,LOW); // turn off light

        //convert first 4byte array into its integer value (0 - 10,000) and scale back
        inputint[0] = toInt(&msgIn[3]); // send the int starting at offset 3
        inputint[1] = toInt(&msgIn[7]); // send the int starting at offset 3

        for(int i = 0; i < 2; i++){
          received_setpoint[i] = inputint[i] / 100.0;
          //          Serial.print("SetPoint "); 
          //          Serial.print(i); 
          //          Serial.print(": "); 
          //          Serial.print(received_setpoint[i]);
          //          Serial.println(" :: ");
        }
        Serial.println("");
      }

      Setpoint = received_setpoint[0];
      Setpoint2 = received_setpoint[1];
      //      myPID.Compute();
      //      myPID2.Compute();
      //      writeval = int(Output);
      //      writeval2 = int(Output2);
      //      analogWrite(ACT1,writeval);
      //      analogWrite(ACT2,writeval);

    }
    else{
      //Serial.println("No msg received"); 
    }

    myPID.Compute();
    myPID2.Compute();
    writeval = int(Output);
    writeval2 = int(Output2);
    analogWrite(ACT1,writeval);
    analogWrite(ACT2,writeval);

    /*******
     *SEND
     ******/
    msgOut[0] = 0;
    msgOut[1] = currentActPosition;
    //    msgOut[2] = sensorValue2;
    msgOut[2] = currentAct2Position;
    msgOut[3] = map(Output,5,250,1,125);
    msgOut[4] = map(Output2,5,250,1,125);  
    acc.write(msgOut, 5);
    //    delay(DelayValue);

    //Serial.println(sensorValue, HEX);

    //delay(50); 

  }
  else{
    //Serial.println("Acc unsuccessful on connection"); 
  }
}

int toInt(byte in[]){
  int a[4];

  for(int i = 0; i < 4; i++){
    a[i] = int(in[i]);
  }
  return a[0] << 24 | a[1] << 16 | a[2] << 8 | a[3]; // bit shift and or together
}

void ramp(int direction, int actNumber){ // direction = 1: open, direction = -1: close  actNumber = 1,2

  int STEPS = 100;
  if(actNumber == 1){
    /*****************************  If in actual Control of Actuator 1
     * for(int i = 0; i < STEPS/3; i++){ // Use < instead of <= ~~guess is that actuator doesn't like a PWM of 0% or 100%
     * analogWrite(ACT1, 127 + i * direction * 127 / STEPS);
     * delay(2000/STEPS); // 2 seconds total time
     * }
     * delay(500); // delay for testing
     *********************************/

    while(true){ // use this loop if encoder range is determined by switch
      digitalWrite(LED1, HIGH);
      if(digitalRead(SWITCH)){
        break;
      }
    }
  }

  if(actNumber == 2){
    /*****************************  If in actual Control of Actuator 2
     * for(int i = 0; i < STEPS/3; i++){ // Use < instead of <= ~~guess is that actuator doesn't like a PWM of 0% or 100%
     * analogWrite(ACT2, 127 + i * direction * 127 / STEPS);
     * delay(2000/STEPS); // 2 seconds total time
     * }
     * delay(500); // delay for testing
     ******************************/
    while(true){ // use this loop if encoder range is determined by switch
      digitalWrite(LED2, HIGH);
      if(digitalRead(SWITCH2)){
        break;
      }
    }
  }
}
























